/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "J2B2.h"
#include "J2B2_settings.h"
#include "World.h"
#include "owndebug.h"
#include "Protocol.h"

CObject* CJ2B2::LoadMLMA(CWorld *world, xmlNode *node)
{
  CObject *object=NULL;
  // Parameters for a Line.
  xmlChar *x = xmlGetProp(node, (xmlChar *) "x");
  xmlChar *y = xmlGetProp(node, (xmlChar *) "y");
  xmlChar *heading = xmlGetProp(node, (xmlChar *) "heading");
  xmlChar *name = xmlGetProp(node, (xmlChar *) "name");
  xmlChar *group = xmlGetProp(node, (xmlChar *) "MaCIGroup");

  bool camera=false;
  
  if (xmlGetProp(node,(xmlChar *)"camera"))
  {
    if (!(xmlStrcmp(xmlGetProp(node,(xmlChar *)"camera"),(xmlChar*)"enabled"))
        || !(xmlStrcmp(xmlGetProp(node,(xmlChar *)"camera"),(xmlChar*)"enable")))
    {
      camera=true;
      dPrint(1,"Using camera in J2B2!");
    }
  }
  
  // Process.
  if (x && y && heading && (name || group)) {
    object=new CJ2B2(world,
                    atof((const char*)x),
                    atof((const char*)y),
                    atof((const char*)heading),
                    name!=NULL ? (const char*)name : "",
                    group!=NULL ? (const char*)group : "",
                    camera);
  } else {
    dPrint(1,"MLMA Error:Not enough parameters defined for J2B2-object!");
  }

  // Cleanup (Always do this to clean out properties which were found)
  if (x) xmlFree(x);
  if (y) xmlFree(y);
  if (heading) xmlFree(heading);
  if (name) xmlFree(name);
  if (group) xmlFree(group);
  
  if (object)
  {
    world->InsertObject(object);
  }
  
  return object;
}

int CJ2B2::refCount=0;

CJ2B2::CJ2B2(CWorld* World, float x, float y, float heading, std::string name, 
             std::string aMaCIGroup, bool useCamera=false)
  : CObject(World,NULL),
    name(name),
    MaCIGroup(aMaCIGroup),
    useCamera(useCamera),
    gimi(),
    mcs(&gimi),
    SpeedCtrlServer(&mcs,0),
    PositionServer(&mcs,0),
    RangingServer(&mcs,MINOR_TYPE_BUMPER),
    EnergyServer(&mcs,0),
    iSpeed(0.0),
    iAngularSpeed(0.0),
    transX(0),
    transY(0),
    transHeading(0),
    laserTimer(0),
    positionTimer(0),
    energyTimer(0),
    bumperHit(true),
    stepsSinceHit(0),
    energyLevel(50.0),energyDiff(1)
{
#warning "NOTE: Using inverted y-axis and heading"
  y=-y;
  heading=-heading;
  dPrint(1,"Base %s",World->GetBaseMaCIGroup().c_str());
  dPrint(1,"MaCIGroup %s",MaCIGroup.c_str());
  if (MaCIGroup.length())
  {
    mcs.SetGroupName(World->GetBaseMaCIGroup()+"."+MaCIGroup);

  } else {
    mcs.SetGroupName(World->GetBaseMaCIGroup()+"."+name);

  }
  
  if (name.length())
  {
    
  } else {
    name=MaCI::GetStaticClientName(MaCIGroup+"FSRSimJ2B2");
  }
  
  SpeedCtrlServer.SetInterfaceInstanceName("Motion");
  PositionServer.SetInterfaceInstanceName("Motion");
  RangingServer.SetInterfaceInstanceName("Bumpers");
  EnergyServer.SetInterfaceInstanceName("Energy");
  /*
    #warning Also; Accepting authentication keys would be nice :)
    unsigned char authkey[16] = "r6oZE93Ba1Q6KKn";
    dPrint(1,"Setting Authentication to '%s', 1 simultaneous access, 10seconds timeout. HARDCODED!",authkey);
    SpeedCtrlServer.SetServerInterfaceAuthentication(MaCI::MaCICtrl::TAuthentication(authkey,
    16),
    1,
    10000);
  */

  /*
  init j2b2 model by creating individual parts, placing them on right positions
  and finally attaching them with joints
  */

  //materials
  WheelMaterial.look.color[0]=0.2;WheelMaterial.look.color[1]=0.2;WheelMaterial.look.color[2]=0.2;

  HullMaterial.look.color[0]=0.2;HullMaterial.look.color[1]=0.2;HullMaterial.look.color[2]=0.8;
  HullMaterial.density=800.0; //just a guess
  HullMaterial.surface.mu=1.0;

  BumperMaterial.look.color[0]=0.8;BumperMaterial.look.color[1]=0.2;BumperMaterial.look.color[2]=0.2;
  BumperMaterial.surface.mu=1.0;

  //objects
  //primitives
  Parts.push_back(Hull=new CCylinder(HULL_RADIUS,HULL_HEIGHT,this,this->SpaceID,0,&HullMaterial,false));
  Parts.push_back(Bumper=new CCylinder(BUMPER_RADIUS,BUMPER_THICKNESS,this,this->SpaceID,Hull->BodyID,&BumperMaterial,false));
  Parts.push_back(RWheel=new CCylinder(DRIVING_WHEEL_RADIUS,DRIVING_WHEEL_WIDTH,this,this->SpaceID,0,&WheelMaterial,false));
  Parts.push_back(LWheel=new CCylinder(DRIVING_WHEEL_RADIUS,DRIVING_WHEEL_WIDTH,this,this->SpaceID,0,&WheelMaterial,false));
  Parts.push_back(FrontCaster=new CSphere(CASTER_WHEEL_RADIUS,this,this->SpaceID,0,&WheelMaterial,false));
  Parts.push_back(RearCaster=new CSphere(CASTER_WHEEL_RADIUS,this,this->SpaceID,0,&WheelMaterial,false));

  collider=new CSphere(HULL_RADIUS,this,this->SpaceID,Hull->BodyID,&HullMaterial,false);

  Bumper->NotifyCollisions=true;

  dReal pos[3]={SCANNER_OFFSET_X,0,SCANNER_OFFSET_Z};
  dMatrix3 rot;
  dRFrom2Axes(rot, 0,1,0, 0,0,1);

  //devices
  Children.push_back(LaserScanner=new CSick(World,this,Hull->BodyID,pos,rot,LASER_INTERVAL,&mcs,MINOR_TYPE_LASER,"Laser"));
  LaserScanner->SetDevicePosition(SCANNER_OFFSET_X,0,SCANNER_OFFSET_Z,0);
  
  pos[0]=CAMERA_OFFSET_X;
  pos[1]=0;
  pos[2]=CAMERA_OFFSET_Z;
  Children.push_back(camera=new CCamera(World,this,Hull->BodyID,
                     pos,rot,320,240,70,&gimi,&mcs,"CameraFront", "ServoCtrl", 
                     !(useCamera) ));

  //placement and rotation of parts
  //dMatrix3 R;
  const dReal *P,*RR;

  
  setPose(x,y,0,DegToRad(heading));

  //joints

  FCasterJoint=dJointCreateBall(this->WorldID,0);
  RCasterJoint=dJointCreateBall(this->WorldID,0);
  LMotor=dJointCreateHinge(this->WorldID,0);
  RMotor=dJointCreateHinge(this->WorldID,0);
  //dPrint(1,"RMotor=%d, LMotor=%d",RMotor,LMotor);
  dJointAttach(FCasterJoint,Hull->BodyID,FrontCaster->BodyID);
  dJointAttach(RCasterJoint,Hull->BodyID,RearCaster->BodyID);
  dJointAttach(LMotor,Hull->BodyID,LWheel->BodyID);
  dJointAttach(RMotor,Hull->BodyID,RWheel->BodyID);

  P=dBodyGetPosition(FrontCaster->BodyID);
  dJointSetBallAnchor(FCasterJoint,P[0],P[1],P[2]);
  P=dBodyGetPosition(RearCaster->BodyID);
  dJointSetBallAnchor(RCasterJoint,P[0],P[1],P[2]);

  dReal d[3]={0,0,1};
  dReal a[3];
  P=dBodyGetPosition(RWheel->BodyID);
  RR=dBodyGetRotation(RWheel->BodyID);
  RotateVector(RR,d,a);
  dJointSetHingeAxis(RMotor,a[0],a[1],a[2]);
  dJointSetHingeAnchor(RMotor,P[0],P[1],P[2]);

  P=dBodyGetPosition(LWheel->BodyID);
  RR=dBodyGetRotation(LWheel->BodyID);
  d[2]=-1;
  RotateVector(RR,d,a);
  dJointSetHingeAxis(LMotor,a[0],a[1],a[2]);
  dJointSetHingeAnchor(LMotor,P[0],P[1],P[2]);

  
  dJointSetHingeParam(RMotor, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetHingeParam(LMotor, dParamFMax, AXLE_FRICTION_TORQUE);
  
}

CJ2B2::~CJ2B2()
{
  Deactivate();
  delete collider;
}

void CJ2B2::Activate()
{
  connectGIMI();
  
  MaCI::EMaCIError e=SpeedCtrlServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open SpeedCtrlServer");
  } else {
    SpeedCtrlServer.SetSpeedLimits(MAX_MOTOR_SPEED, MAX_ANGULAR_SPEED, MAX_ACCELERATION,
                                   -MAX_MOTOR_SPEED, -MAX_ANGULAR_SPEED, -MAX_ACCELERATION);
  }
  e=PositionServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open PositionServer");
  }
  e=RangingServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open RangingServer");
  }
  RangingServer.SetDeviceLimits(-1,BUMPER_RADIUS);
  
  e=EnergyServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open EnergyServer");
  }
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Activate();
  }
}

void CJ2B2::Deactivate()
{
  EnergyServer.Close();
  RangingServer.Close();
  PositionServer.Close();
  SpeedCtrlServer.Close();

  if (LaserScanner) LaserScanner->Deactivate();
  if (camera) camera->Deactivate();
  //  CObject::Deactivate();

  gimi.stop();
}


void CJ2B2::Update(float StepSize)
{
  //dPrint(1,"Update start");
  //get latest speed command skipping queued commands
  if (SpeedCtrlServer.WaitCommand(SpeedCtrlData,0,true)==MaCI::KMaCIOK)
  {
    const MaCI::SpeedCtrl::TCommand *c=SpeedCtrlData.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c)
      switch (c->cmd)
        {
        case MaCI::SpeedCtrl::KCommandSetSpeed:
          s=SpeedCtrlData.GetSpeed();
          if (s)
            SetSpeed(s->speed,s->angularSpeed,s->acceleration);
          break;
        case MaCI::SpeedCtrl::KCommandSetStop:
          SetSpeed(0,0,1);
          break;
        case MaCI::SpeedCtrl::KCommandTimeout:
          SetSpeed(0,0,1);
          break;
        }
 
    
  }

  //float t=SimWorld->GetSimulationTime();
  MaCI::SpeedCtrl::CSpeedCtrlData speedEvent;
  gim::binbag::CBinBag bag;
  speedEvent.EncodeTo(&bag); 
  speedEvent.SetCommand(MaCI::SpeedCtrl::TCommand(MaCI::SpeedCtrl::KEventSpeed));
  speedEvent.SetSpeed(MaCI::SpeedCtrl::TSpeed(iSpeed,iAngularSpeed));
  //speedEvent.SetTimestamp(MaCI::Common::TTimestamp((int)floorf(t),(int)((t-floorf(t))*1000000)));
  speedEvent.SetTimestamp(MaCI::Common::TTimestamp());
  SpeedCtrlServer.SendSpeedEvent(speedEvent);
  
  //get repos commands
  gimi::GIMIMessage msg;
  if (gimi.receive(msg,0,GIMI_PROTOCOL_FSRSIM_OBJECT_REPOSITION,GIMI_IGNOREMINORTYPE)==GIMI_OK)
  {
    TFSRSimObjectRepositionCmd repos;
    if (repos.parseData(msg.data,msg.datalength))
    {
      dPrint(1,"reposing to %f %f %f %f",repos.x,repos.y,repos.z,repos.heading);
#warning "NOTE: Using inverted y-axis and heading"
      setPose(repos.x,-repos.y,repos.z,-repos.heading);
    }
  }
  

  //update child objects
  //dPrint(1,"Updating children");
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Update(StepSize);
  }

  //update devices
  //if bumper isnt hitting wall anymore, send resetting data
  if (bumperHit)
  {
    stepsSinceHit++;
    if (stepsSinceHit>=2)
    {
      bumperHit=false;
      float distances[4]={-1, -1, -1 , -1};
      float angles[4]={0, M_PI/2 ,M_PI ,-M_PI/2};
      //float t=SimWorld->GetSimulationTime();
      RangingServerData.Reset();
      RangingServerData.EncodeTo(&binbag);
      //RangingServerData.SetTimestamp(MaCI::Common::TTimestamp((int)floorf(t),(int)((t-floorf(t))*1000000)));
      RangingServerData.SetTimestamp(MaCI::Common::TTimestamp());
      RangingServerData.SetDistanceHeader(MaCI::Ranging::TDistanceHeader(0,4));
      MaCI::Ranging::TDistance d;
      for (int i=0;i<4;i++)
      {
        d.angle=angles[i];
        d.distance=distances[i];
        RangingServerData.AddDistance(d);
      }
      RangingServer.SendDistanceArrayEvent(RangingServerData);
    }
  }


  MaCI::Position::CPositionData pos;
  
  if (PositionServer.WaitCommand(pos,0)==MaCI::KMaCIOK)
  {
    const MaCI::Position::TCommand *c=pos.GetCommand();
    if (c && c->cmd==MaCI::Position::KCommandSetPose2D)
    {
      //SetOdoPosition(pos.GetPose2D()->x,pos.GetPose2D()->y,pos.GetPose2D()->a);
    }
  }
  
  if (positionTimer>=POSITION_INTERVAL)
  {
    const dReal *P, *R;
    dReal v[3]={1,0,0};
    dReal r[3];
    float heading;
    P=dBodyGetPosition(Hull->BodyID);
    R=dBodyGetRotation(Hull->BodyID);
    RotateVector(R,v,r);
#warning "NOTE: Using inverted y-axis and heading"
    heading=atan2(-r[2],r[0]);
    /*
    odoPosX=P[0]-offsetX;
    odoPosY=-P[2]-offsetY;
    odoPosZ=P[1]-offsetZ;
    odoHeading=heading-offsetHeading;*/
    calculateOdoPose();

    //float t=SimWorld->GetSimulationTime();
    MaCI::Position::CPositionData pd;
      
    pd.EncodeTo(&binbag);
    pd.SetTimestamp(MaCI::Common::TTimestamp());
    //dPrint(1,"odoposx=%f odoposy=%f odoheading=%f", odoPosX, odoPosY, odoHeading);
    MaCI::Position::TPose2D pose(odoPosX,odoPosY,odoHeading);
    pose.NormalizeAngle();
   
    pd.SetPose2D(pose);
    pd.SetVariance2D(MaCI::Position::TVariance2D(0,0,0));
    pd.SetProbability(MaCI::Position::TProbability(1));

    PositionServer.SendPositionEvent(pd);

    positionTimer=0;
  }
  positionTimer+=StepSize;
  //dPrint(1,"Update end");

  if (energyTimer>=ENERGY_INTERVAL)
  {
    MaCI::Energy::CEnergyData ed;
    ed.CreateInternalBinBag();
    ed.SetTimestamp(MaCI::Common::TTimestamp());
    // Add a dummy event
    ed.SetEnergyEvent(MaCI::Energy::TEnergyEvent(MaCI::Energy::KEventTypeEnergyData, energyLevel, 100.0, 1.0));
      
    if (energyLevel==0)
    {
      energyDiff=1;
    } else if (energyLevel == 100) {
      energyDiff=-1;
    }
      
    energyLevel=energyLevel+energyDiff;
      
      // Broadcast!
    EnergyServer.SendEnergyEvent(ed);
    
    energyTimer=0;
  }
  energyTimer+=StepSize;
  
}

void CJ2B2::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{
  if (primitive==Bumper)
  {
    bumperHit=true;
    stepsSinceHit=0;

    const dReal *P,*R;
    dReal X[3]={1, 0, 0};
    dReal Y[3]={0, 1, 0};
    dReal x[3];
    dReal y[3];
    P=dBodyGetPosition(Hull->BodyID);
    R=dBodyGetRotation(Hull->BodyID);
    RotateVector(R,X,x);
    RotateVector(R,Y,y);

    dReal cpos[3]={contacts[0].geom.pos[0]-P[0], contacts[0].geom.pos[1]-P[1], contacts[0].geom.pos[2]-P[2]};
    float Cos=DotProduct(x,cpos);
    float Sin=DotProduct(y,cpos);
    float angle=atan2(Sin,Cos);

    float distances[4]={-1, -1, -1 , -1};

//#ifndef USE_MACI
    float angles[4]={0, M_PI/2 ,M_PI ,-M_PI/2};
//#endif

    float p=M_PI/8;

    if (angle<=p && angle >=-p) { //north
      distances[0]=BUMPER_RADIUS;
    } else if (angle>p && angle<=3*p ) { //nw
      distances[0]=BUMPER_RADIUS;
      distances[1]=BUMPER_RADIUS;
    } else if (angle>3*p && angle<=5*p ) { //west
      distances[1]=BUMPER_RADIUS;
    } else if (angle>5*p && angle<=7*p ) { //sw
      distances[1]=BUMPER_RADIUS;
      distances[2]=BUMPER_RADIUS;
    } else if (angle>=7*p || angle<=-7*p ) { //south
      distances[2]=BUMPER_RADIUS;
    } else if (angle<-p && angle>=-3*p ) { //ne
      distances[0]=BUMPER_RADIUS;
      distances[3]=BUMPER_RADIUS;
    } else if (angle<-3*p && angle>=-5*p ) { //east
      distances[3]=BUMPER_RADIUS;
    } else /*if (angle<-5*p && angle>=-7*p )*/ { //se
      distances[3]=BUMPER_RADIUS;
      distances[2]=BUMPER_RADIUS;
    }
    //float t=SimWorld->GetSimulationTime();
    RangingServerData.Reset();
    RangingServerData.EncodeTo(&binbag);
//    RangingServerData.SetTimestamp(MaCI::Common::TTimestamp((int)floorf(t),(int)((t-floorf(t))*1000000)));
    RangingServerData.SetTimestamp(MaCI::Common::TTimestamp());
    RangingServerData.SetDistanceHeader(MaCI::Ranging::TDistanceHeader(0,4));
    MaCI::Ranging::TDistance d;
    for (int i=0;i<4;i++)
    {
      d.angle=angles[i];
      d.distance=distances[i];
      RangingServerData.AddDistance(d);
    }
    /*dPrint(1,"sent bumper event %f %f %f %f",(RangingServerData.GetDistance(0))->distance,
        (RangingServerData.GetDistance(1))->distance,(RangingServerData.GetDistance(2))->distance,
        (RangingServerData.GetDistance(3))->distance);*/
    //dPrint(1,"numofd %d",RangingServerData.GetDistanceElementCount());
    RangingServer.SendDistanceArrayEvent(RangingServerData);

  }
}

void CJ2B2::Draw()
{
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Draw();
  }

  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++)
  {
    (*i)->Draw();
  }

}

void CJ2B2::SetSpeed(float speed, float angularSpeed, float acceleration)//, float angularAcceleration)
{
  float leftVelocity = 0.0;
  float rightVelocity = 0.0;
  float ar, al;
  float r = WHEELBASE/2.0;
  float f;
  dMass mass;

  rightVelocity = angularSpeed*r+speed;
  leftVelocity = 2.0*speed-rightVelocity;

  if (leftVelocity >= -MAX_MOTOR_SPEED && leftVelocity <= MAX_MOTOR_SPEED &&
      rightVelocity >= -MAX_MOTOR_SPEED && rightVelocity <= MAX_MOTOR_SPEED)
  {
    ar=rightVelocity/(DRIVING_WHEEL_RADIUS);
    al=-leftVelocity/(DRIVING_WHEEL_RADIUS); //invert this speed

    dJointSetHingeParam(this->RMotor, dParamVel, ar);
    dJointSetHingeParam(this->LMotor, dParamVel, al);

    //calculate acceleration
    dBodyGetMass(this->Hull->BodyID,&mass);
    if (acceleration!=0)
    {
      //TODO: how to actually do this?
      f=fabs(mass.mass*acceleration)*DRIVING_WHEEL_RADIUS;
    }else{
      f=0;
    }

    dJointSetHingeParam (RMotor, dParamFMax, f);
    dJointSetHingeParam (LMotor, dParamFMax, f);
    iSpeed = speed;
    iAngularSpeed = angularSpeed;
  } else {
    dPrint(1,"Speeds out of range: %f or %f",
	  leftVelocity, rightVelocity);
  }
}

void CJ2B2::SetOdoPosition(float x, float y, float heading)
{
  const dReal *P, *R;
  dReal v[3]={1,0,0};
  dReal r[3];
  float h;
  P=dBodyGetPosition(Hull->BodyID);
  R=dBodyGetRotation(Hull->BodyID);
  RotateVector(R,v,r);
#warning "NOTE: Using inverted y-axis and heading"
  h=atan2(-r[2],r[0]);
  transX=P[0];
  transY=-P[2];
  transZ=P[1];
  transHeading=heading-h;
  odoPosX=x;
  odoPosY=y;
  odoPosZ=0;
  odoHeading=heading;
}

void CJ2B2::calculateOdoPose()
{
  const dReal *P, *R;
  dReal v[3]={1,0,0};
  dReal r[3];
  float h;
  P=dBodyGetPosition(Hull->BodyID);
  R=dBodyGetRotation(Hull->BodyID);
  RotateVector(R,v,r);
#warning "NOTE: Using inverted y-axis and heading"
  h=atan2(-r[2],r[0]);
  
  odoPosX=cos(transHeading)*P[0]+sin(transHeading)*-P[2]-transX;
  odoPosY=-sin(transHeading)*P[0]+cos(transHeading)*-P[2]-transY;
  odoHeading=transHeading+h;
}

void CJ2B2::connectGIMI()
{
  if (SimWorld->ConnectGimi(&gimi,name))
    gimi.addAcceptedService(GIMI_PROTOCOL_FSRSIM_OBJECT_REPOSITION,GIMI_IGNOREMINORTYPE,"Reposition");
}


void CJ2B2::setPose(float x, float y, float z, float heading)
{
   
  dMatrix3 R;

  
  dRFrom2Axes(R,cos(heading), 0, sin(heading),
              sin(heading), 0, -cos(heading));
  dBodySetRotation(Hull->BodyID,R);
  dBodySetPosition(Hull->BodyID,x,HULL_Y,y);

  dBodySetPosition(RWheel->BodyID, x+cos(heading+DegToRad(90))*DRIVING_WHEEL_X,
                DRIVING_WHEEL_RADIUS, y+sin(heading+DegToRad(90))*DRIVING_WHEEL_X);
  dBodySetPosition(LWheel->BodyID, x-cos(heading+DegToRad(90))*DRIVING_WHEEL_X,
                DRIVING_WHEEL_RADIUS, y-sin(heading+DegToRad(90))*DRIVING_WHEEL_X);
  dRSetIdentity(R);
  dRFromAxisAndAngle(R,0,1,0,-heading);
  dBodySetRotation(RWheel->BodyID,R);
  dBodySetRotation(LWheel->BodyID,R);


  dBodySetPosition(FrontCaster->BodyID, x+cos(heading)*CASTER_WHEEL_Z,
  CASTER_WHEEL_RADIUS+CASTER_WHEEL_Y, y+sin(heading)*CASTER_WHEEL_Z);
  dBodySetPosition(RearCaster->BodyID, x-cos(heading)*CASTER_WHEEL_Z,
  CASTER_WHEEL_RADIUS+CASTER_WHEEL_Y, y-sin(heading)*CASTER_WHEEL_Z);
  
}



